package novemberBot.sensor;

import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.util.Timer;
import lejos.util.TimerListener;

/**polles the sensors non stop and passes the data to the responders that currently want it with the filter they want
 * @author Antoine Lennasrtz*/
public class UsPoller implements TimerListener{
	/**default timer delay*/
	private static final int TIMER_DELAY=60;
	/**high mounted sensor*/
	private MotorisedUsS usHigh;
	
	/**last data from high sensor*/
	private double[] lastHighData;
	
	/**low mounted sensor*/
	private MotorisedUsS usLow;
	
	/**last data from low sensor*/
	private double[] lastLowData;
	
	/**Timer to synchronise pings*/
	private Timer t;
	/**array containing responders*/
	private UsDataResponder[] responders= new UsDataResponder[0];
	
	/**Constructor, auto starts polling
	 * @param highSPort - sensor port for high sensor
	 * @param lowSPort - sensor port for low sensor
	 * @param mPort - motor port for sensors
	 * @see UsPoller#startPolling()*/
	public UsPoller(MotorPort mPort, SensorPort highSPort, SensorPort lowSPort){
		usHigh = new MotorisedUsS(mPort, highSPort, 1, true);
		usLow = new MotorisedUsS(mPort, lowSPort, 1, true);
		t=new Timer(TIMER_DELAY, this);
		startPolling(); 	
	}
	/**Starts the polling timer*/
	public void startPolling(){
		usHigh.constantSweep(new int[]{-30, 30}, 110);
		t.start();
	}
	/**pauses Sweeping of sensors*/
	public void pauseSweep(){
		usHigh.pauseSweep();
	}
	/**resumes Sweeping of sensors*/
	public void resumeSweep(){
		usHigh.resumeSweep();
	}
	/**adds a resonder to the responders array
	 * @param responder - new responder*/
	public void addResponder(UsDataResponder responder){
		UsDataResponder[] resp = new UsDataResponder[responders.length+1];
		for(int i=0; i<responders.length; i++){
			resp[i]=responders[i];
		}
		resp[responders.length]=responder;
		responders=resp;
	}
	
	/**uses alternate sensor pings to avoid interference
	 * writes data to class variables*/
	@Override
	public void timedOut() {
		lastHighData=usHigh.getDistnPos();
		lastHighData[0]=modelHigh(lastHighData[0]);
		lastLowData=usLow.getDistnPos();
		lastLowData[0]=modelLow(lastLowData[0]);
		passToResponders();
	}
	
	/**passes newly acquired data to responders.
	 * more specifically to the responder's filter object
	 * from where the responder actually gets it*/
	private void passToResponders(){
		for(int i=0; i<responders.length; i++){
			if(responders[i].needsData){
				if(responders[i].s==UsDataResponder.Sensor.HIGH){
					responders[i].newUsData(lastHighData);
				} else{
					responders[i].newUsData(lastLowData);
				}
			}
		}
	}
	
	//accessors
	/**low sensor accessor*/
	public MotorisedUsS getUsLow(){
		return usLow;
	}
	/**high sensor accessor*/
	public MotorisedUsS getUsHigh(){
		return usHigh;
	}
	/**models sensor data
	 * @param d - data to be modeled
	 * @return modeled value according to High sensor modeling tests*/
	private static double modelHigh(double d){
		if(d<=14)
			return (0.0663*d*d-0.6635*d+8.0758);
		else if(d<=23)
			return 0.1354*d*d-4.2067*d+46.921;
		else
			return (0.9955*d-0.0025);
	}
	/**models sensor data
	 * @param d - data to be modeled
	 * @return modeled value according to Low sensor modeling tests*/
	private static double modelLow(double d){
		if(d<=16)
			return (0.6154*d+0.1923);
		else if(d<=25)
			return d;
		else 
			return d-2;
	}
	
}
